clc;
clear;
% close all;

glvs
psinstypedef(186);
trj = trjfile('trj10ms.mat');
[nn, ts, nts] = nnts(1, trj.ts);
%% init
imuerr = imuerrset(30, 2000, 0.15, 100);
% imu = imuadderr(trj.imu, imuerr);
davp0 = avperrset([30;-30;90], [0.15;0.15;0.15], [1;1;3]);
lever = [5; 5; 2]*0;
% gps = gpssimu(trj.avp, davp0(4:6), davp0(7:9), 1, lever, 0, 1);
% save("imugps.mat",'gps','imu');
load('imugps.mat');
imugpssyn(imu(:,7), gps(:,7));
ins = insinit(trj.avp0(1:9), ts, davp0);

avpref = trj.avp;
save("avpref.mat",'avpref');

ki = 1;
len = length(imu);
out.imu = [imu(:,1:3)*180/pi/ts, imu(:,4:6)/ts, imu(:,7)];
out.gnss = zeros(len,7);
for k = 1:len
    if(mod(k,100) == 0 & ki < length(gps))
        out.gnss(k,:) = gps(ki,:);
        ki = ki + 1;
    end
end
out.imugps = [out.imu,out.gnss];
outFilePath = "D:\STM32_Workspace\iNAV_STM32_PRJ\iNav_lib\matlab\";
writematrix(out.imugps, outFilePath+"imugps.txt", 'Delimiter', ',', 'WriteMode', 'overwrite');

%% kf
r0 = vperrset(0.1, [1,1,3]);
imuerr.web = imuerr.web * 1.0;
imuerr.wdb = imuerr.wdb * 1.0;
kf = kfinit(ins, davp0, imuerr, lever*0, r0);
len = length(imu); [avp, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);
timebar(nn, len, '18-state SINS/GPS simulation.'); ki = 1;
for k=1:nn:len-nn+1
    k1 = k+nn-1; 
    wvm = imu(k:k1,1:6); t = imu(k1,end);
    ins = insupdate(ins, wvm);
    kf.Phikk_1 = kffk(ins);
    kf = kfupdate(kf);
    [kgps, dt] = imugpssyn(k, k1, 'F');
    if kgps>0
        vnGPS = gps(kgps,1:3)'; posGPS = gps(kgps,4:6)';
        ins = inslever(ins, lever);
        kf.Hk = kfhk(ins);
        zk = [ins.vnL-ins.an*dt-vnGPS; ins.posL-ins.Mpvvn*dt-posGPS];
        kf = kfupdate(kf, zk, 'M');
        [kf, ins] = kffeedback(kf, ins, 1, 'avp');
        avp(ki,:) = [ins.avp', t];
        xkpk(ki,:) = [kf.xk; diag(kf.Pxk); t]';  ki = ki+1;
    end
    timebar;
end
avp(ki:end,:) = []; xkpk(ki:end,:) = [];
save("pcAvpOut.mat",'avp');
avperr = avpcmpplot(trj.avp, avp);
kfplot(xkpk, avperr, imuerr, lever);
insplot(avp);

